﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MutsiEngine
{
    public struct Quaternion
    {
        public float W, X, Y, Z;
        public Vector3 EulerAngles
        {
            get { return getEuler(false); }
            set { eulerAngles = value; SetEuler(value); }
        }
        private Vector3 eulerAngles;

        private const float PIOVER180 = 0.0174532925f;

        public static Quaternion Identity
        {
            get { return new Quaternion(1f, 0f, 0f, 0f); }
        }

        public Quaternion(float w, float x, float y, float z)
        {
            W = w;
            X = x;
            Y = y;
            Z = z;
            eulerAngles = Vector3.Zero;
        }

        public void Normalize()
        {
            float magnitude = (float)Math.Sqrt((W * W) + (X * X) + (Y * Y) + (Z * Z));
            W = W / magnitude;
            X = X / magnitude;
            Y = Y / magnitude;
            Z = Z / magnitude;
        }

        // from 3 euler angles 
        public static Quaternion FromEuler(Vector3 angles)
        {
            float cos_z_2 = (float)Math.Cos(0.5f * angles.Z);
            float cos_y_2 = (float)Math.Cos(0.5f * angles.Y);
            float cos_x_2 = (float)Math.Cos(0.5f * angles.X);

            float sin_z_2 = (float)Math.Sin(0.5f * angles.Z);
            float sin_y_2 = (float)Math.Sin(0.5f * angles.Y);
            float sin_x_2 = (float)Math.Sin(0.5f * angles.X);

            // and now compute quaternion
            Quaternion quat = new Quaternion();
            quat.W = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2;
            quat.X = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2;
            quat.Y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2;
            quat.Z = sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2;

            return quat;
        }

        public void SetEuler(Vector3 angles)
        {
            float cos_z_2 = (float)Math.Cos(0.5f * angles.Z);
            float cos_y_2 = (float)Math.Cos(0.5f * angles.Y);
            float cos_x_2 = (float)Math.Cos(0.5f * angles.X);

            float sin_z_2 = (float)Math.Sin(0.5f * angles.Z);
            float sin_y_2 = (float)Math.Sin(0.5f * angles.Y);
            float sin_x_2 = (float)Math.Sin(0.5f * angles.X);

            W = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2;
            X = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2;
            Y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2;
            Z = sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2;
        }

        public static Quaternion FromAngleAxis(float angle, Vector3 axis)
        {
            Quaternion quat = new Quaternion();

            float halfAngle = 0.5f * angle;
            float sin = (float)Math.Sin(halfAngle);

            quat.W = (float)Math.Cos(halfAngle);
            quat.X = sin * axis.X;
            quat.Y = sin * axis.Y;
            quat.Z = sin * axis.Z;

            return quat;
        }

        //! returns the euler angles from a rotation quaternion
        private Vector3 getEuler(bool homogenous)
        {
            float sqw = W * W;
            float sqx = X * X;
            float sqy = Y * Y;
            float sqz = Z * Z;

            Vector3 euler;
            if (homogenous)
            {
                euler.X = (float)Math.Atan2(2f * (X * Y + Z * W), sqx - sqy - sqz + sqw);
                euler.Y = (float)Math.Asin(-2f * (X * Z - Y * W));
                euler.Z = (float)Math.Atan2(2f * (Y * Z + X * W), -sqx - sqy + sqz + sqw);
            }
            else
            {
                euler.X = (float)Math.Atan2(2f * (Z * Y + X * W), 1 - 2 * (sqx + sqy));
                euler.Y = (float)Math.Asin(-2f * (X * Z - Y * W));
                euler.Z = (float)Math.Atan2(2f * (X * Y + Z * W), 1 - 2 * (sqy + sqz));
            }
            return euler;
        }

        /* Calculates and returns a rotation matrix created from this Quaternion
         * This function has partially been copied from The Axiom3D engine (http://axiom3d.net/)
         * Axiom3d is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public http://www.gnu.org/licenses/lgpl-2.1.html
         */
        public double[] GetMatrix()
        {
            double[] matrix = new double[16];

            double tx = 2.0f * X;
            double ty = 2.0f * Y;
            double tz = 2.0f * Z;
            double twx = tx * W;
            double twy = ty * W;
            double twz = tz * W;
            double txx = tx * X;
            double txy = ty * X;
            double txz = tz * X;
            double tyy = ty * Y;
            double tyz = tz * Y;
            double tzz = tz * Z;

            matrix[0] = 1.0f - (tyy + tzz);
            matrix[1] = txy - twz;
            matrix[2] = txz + twy;

            matrix[4] = txy + twz;
            matrix[5] = 1.0f - (txx + tzz);
            matrix[6] = tyz - twx;

            matrix[8] = txz - twy;
            matrix[9] = tyz + twx;
            matrix[10] = 1.0f - (txx + tyy);

            matrix[15] = 1.0f;

            return matrix;
        }

        public static Quaternion operator *(Quaternion q1, Quaternion q2)
        {
            float nw = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
            float nx = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
            float ny = q1.W * q2.Y + q1.Y * q2.W + q1.Z * q2.X - q1.X * q2.Z;
            float nz = q1.W * q2.Z + q1.Z * q2.W + q1.X * q2.Y - q1.Y * q2.X;
            return new Quaternion(nw, nx, ny, nz);
        }

        public static bool operator ==(Quaternion q1, Quaternion q2)
        {
            return (q1.W == q2.W && q1.X == q2.X && q1.Y == q2.Y && q1.Z == q2.Z);
        }

        public static bool operator !=(Quaternion q1, Quaternion q2)
        {
            return !(q1.W == q2.W && q1.X == q2.X && q1.Y == q2.Y && q1.Z == q2.Z);
        }

        public override bool Equals(object other)
        {
            // Check object other is a Vector3 object
            if (other is Quaternion)
            {
                // Convert object to Vector3
                Quaternion otherQuat = (Quaternion)other;

                // Check for equality
                return otherQuat == this;
            }
            else
            {
                return false;
            }
        }

        public bool Equals(Quaternion other)
        {
            return other == this;
        }

        public override int GetHashCode()
        {
            return
            (
               (int)((W + X + Y + Z) % Int32.MaxValue)
            );
        }
    }
}